/*
* avr_test.c
*
* Created: 3/26/2011 22:35:52
*  Author: morpheus
*/ 

#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <util/delay.h>

#ifndef F_CPU
#define F_CPU 16000000UL
#endif

#include "serial.h"
#include "atmega_adc.h"
#include "sabertooth.h"

#define UART_BAUD_RATE      9600

#define HIGH 0x1
#define LOW  0x0

#define AREF 5

//uint16_t getPulseWidth();
//unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);

int main(void)
{
	char buffer[10];
	//int value_t = 0;
	//uint8_t low, high;
  uint16_t distance = 0;
  float kelvin = 0;

  usart_init();

	
	//ADC
	initADC();
  readADC(1); // calibration of sensor
  _delay_ms(20);
	
	DDRB |= (0<< PB0);
  PORTB |= (0 << PB0);


  // Init the motor controller
  //initSaber();
  
  //send("test length\n", sizeof("test length\n"));
  while(1)
  {
    
    distance = readADC(0) * AREF / 10;
#if _DEBUG
	  itoa(distance,buffer,10);

	  _delay_ms(150);
	  
	  //uart_puts("String stored in SRAM\n");
	  //uart_puts(buffer);
	  //uart_putc('\n');
	  //send("test length\n", sizeof("test length\n"));
	  uart_puts("this is a test\n\r");
    uart_puts("distance: " );
    uart_puts(buffer);
    uart_putc('\n');
    uart_putc('\r');
#endif

    if(distance < 10)
    {
      // STOP
	    all_stop();
    }
    else
    {
      // GO
	    forward(HALF);
    }

/*
    //kelvin = (AREF * readADC(1)) / 1024.0;
    kelvin = AREF * readADC(1);
	  uart_puts("this is a temp\n\r");
    //itoa(kelvin, buffer, 10);
	  sprintf(buffer,"%0d",kelvin);
    uart_puts(buffer);
    uart_putc('\n');
    uart_putc('\r');

	  //uart_putc('c');
	  //uart_putc('\n');
	  
		//test = pulseIn(PB3, HIGH, 14700);
    PORTB &= ~(1 << 1);
	  _delay_ms(1000);
    */
  }
}

/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
 * or LOW, the type of pulse to measure.  Works on pulses from 2-3 microseconds
 * to 3 minutes in length, but must be called at least a few dozen microseconds
 * before the start of the pulse. */
/*unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout)
{
        // cache the port and bit of the pin in order to speed up the
        // pulse width measuring loop and achieve finer resolution.  calling
        // digitalRead() instead yields much coarser resolution.
        uint8_t bit = digitalPinToBitMask(pin);
        uint8_t port = digitalPinToPort(pin);
        uint8_t stateMask = (state ? bit : 0);
        unsigned long width = 0; // keep initialization out of time critical area
        
        // convert the timeout from microseconds to a number of times through
        // the initial loop; it takes 16 clock cycles per iteration.
        unsigned long numloops = 0;
        unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;
        
        // wait for any previous pulse to end
        while ((*portInputRegister(port) & bit) == stateMask)
                if (numloops++ == maxloops)
                        return 0;
        
        // wait for the pulse to start
        while ((*portInputRegister(port) & bit) != stateMask)
                if (numloops++ == maxloops)
                        return 0;
        
        // wait for the pulse to stop
        while ((*portInputRegister(port) & bit) == stateMask)
                width++;

        // convert the reading to microseconds. The loop has been determined
        // to be 10 clock cycles long and have about 16 clocks between the edge
        // and the start of the loop. There will be some error introduced by
        // the interrupt handlers.
        return clockCyclesToMicroseconds(width * 10 + 16); 
}

uint16_t getPulseWidth()
{
  uint32_t i,result;

  //Wait for the rising edge
  for(i=0;i<600000;i++)
  {
    if(!(US_PIN & (1<<0))) 
      continue; 
    else 
      break;
  }

  if(i==600000)
    return 0xffff; //Indicates time out

  //High Edge Found

  //Setup Timer1
  TCCR1A=0X00;
  TCCR1B=(1<<CS11); //Prescaler = Fcpu/8
  TCNT1=0x00;   //Init counter

  //Now wait for the falling edge
  for(i=0;i<600000;i++)
  {
    if(US_PIN & (1<<0))
    {
      if(TCNT1 > 60000) break; else continue;
    }
    else
    break;
  }

  if(i==600000)
    return 0xffff; //Indicates time out

  //Falling edge found

  result=TCNT1;

  //Stop Timer
  TCCR1B=0x00;

  if(result > 60000)
    return 0xfffe; //No obstacle
  else
    return (result>>1);
}*/
